#include <joyos.h>
#include "navigate5.h"
#include "gather.h"

//measurements
#define VPS_PER_FOOT (443.4)
//IO ports
#define GEAR_MOTOR (2)
//motor velocities
#define DRIVE_VELOCITY (150)
#define TURN_VELOCITY (100)
#define SPIN_VELOCITY (250)
//servo positions
#define LEVER_ARM_DOWN (350)
#define LEVER_ARM_UP (100)
#define LEVER_ARM_MIDDLE (175)


//heading parallel to the gearbox wall
uint16_t gatherHeading[6] = {300, 0, 60, 120, 180, 240};

//heading perpendicular to the gearbox wall
uint16_t gatherHeadingFacing[6] = {30, 90, 150, 210, 270, 330};

//heading perpendicular to the sheep dispensor wall
uint16_t sheepHeading[6] = {330, 30, 90, 150, 210, 270};



//Midpoints of each section, points in feet
Point section[6] = { 
	{(1245)/VPS_PER_FOOT, 0},
	{(622.75)/VPS_PER_FOOT, (1078.5)/VPS_PER_FOOT},
	{(-622.75)/VPS_PER_FOOT, (1078.5)/VPS_PER_FOOT},
	{(-1245)/VPS_PER_FOOT, 0},
	{(-622.75)/VPS_PER_FOOT, (-1078.5)/VPS_PER_FOOT},
	{(622.75)/VPS_PER_FOOT, (-1078.5)/VPS_PER_FOOT},
};


//line up for crash into gearbox (run robot perpendicular to wall)
Point gatherTarget[6] = {
	{2.663002  ,0.470186},
	{0.894602  , 2.124161},
	{-1.780883  , 1.941079},
	{-2.791991  , -0.484749},
	{-1.004867  , -2.565220},
	{1.724710  , -2.138724}
};

//in front of the gearbox 
Point gatherTargetClose[6] = {
	{4.043 - (6.*1.73)/12. -1/12., 1 - 6./12. + 1.*1.73/12.},
	{1.155 -2./12., 4 - 12./12.},
	{-2.89 + (6.*1.73)/12. - 1./12., 3 - 6./12. - 1.*1.73/12.},
	{-4.043 + (6.*1.73)/12. + 1./12., -1 + 6./12. - 1.*1.73/12.},
	{-1.155 + 2./12., -4 + 12./12.},
	{ 2.89 - (6.*1.73)/12. + 1./12., -3 + 6./12. + 1.*1.73/12.},
};

Point gatherSheepTargets[6] = {  	// offset by 24 inches away, 3 inch to the right
	{ 4.043- (12.*1.73)/12. - 1.5/12., -1 + 12./12. - 1.5*1.73/12.},
	{ 2.89 - (12.*1.73)/12. + 1.5/12., 3 - 12./12. - 1.5*1.73/12.},
	{-1.115 + 3./12., 4 - 2},
	{-4.043 + (12.*1.73)/12. + 1.5/12., 1 - 12./12. + 1.5*1.73/12.},
	{-2.89 + (12.*1.73)/12. - 1.5/12., -3 + 12./12. + 1.5*1.73/12.},
	{ 1.155 - 3./12., -4 +2},
};

Point gearBoxes[6] = {
	{4.043, 1},
	{1.155 , 4},
	{-2.89, 3},
	{-4.043, -1},
	{-1.155, -4},
	{ 2.89, -3},
};

Point sheepDispensers[6] = {
	{ 4.043, -1},
	{ 2.89, 3},
	{-1.115, 4},
	{-4.043, 1},
	{-2.89, -3},
	{ 1.155, -4},
};

Point getSection(uint8_t territory){
	return section[territory - 1];
}

uint8_t inSection(Point lost){
	uint8_t mySection = 1;
	//printf("\nI need to find a section");
	Point closestPoint = section[0];
	for(uint8_t i = 0; i< 6; i++){
		if(calculateDistance(closestPoint, lost) >= calculateDistance(section[i], lost)){
			closestPoint = section[i];
		}
	}
	if(closestPoint.x == section[0].x && closestPoint.y == section[0].y){
		mySection = 1;
	}else if (closestPoint.x == section[1].x && closestPoint.y == section[1].y){
		mySection = 2;
	}else if (closestPoint.x == section[2].x && closestPoint.y == section[2].y){
		mySection = 3;
	}else if (closestPoint.x == section[3].x && closestPoint.y == section[3].y){
		mySection = 4;
	}else if (closestPoint.x == section[4].x && closestPoint.y == section[4].y){
		mySection = 5;
	}else if (closestPoint.x == section[5].x && closestPoint.y == section[5].y){
		mySection = 6;
	}
		
	return mySection;
}



void captureTerritory(uint8_t territory){
	//printf("\ncapturing territory");

	driveThroughFieldToPoint(gatherTargetClose[territory - 1], 1.0/12.);
	//driveThroughFieldToPoint(section[territory - 1], 1.0/12.);
	
	turnToPoint(gearBoxes[territory - 1]);
	driveForwardFeet(5.0/12., 150);
	
}



void getSheep(uint8_t territory){

	int sheepLeft = getTerritorySheepLeft(territory);
	uint8_t mining = 1;
	uint8_t miningTries = 0;
	uint8_t miningLiningUpTries = 0;
	
	servo_set_pos(1, LEVER_ARM_UP);
	
	if (getTerritoryOwner(territory) == 21){
		driveToPoint(gatherSheepTargets[territory - 1], 1.0/12.);
		//driveToPoint(section[territory -1], 1.0/12.);
		turnToPoint(sheepDispensers[territory-1]);
	
		driveForwardFeet(calculateDistance(sheepDispensers[territory - 1], getMyPosition()) - 5.2/12., 125);
		
		servo_set_pos(1, LEVER_ARM_DOWN);
		pause(500);
		servo_set_pos(1, LEVER_ARM_UP);
		pause(500);	
	}
	
	
	while (getTerritorySheepLeft(territory) == sheepLeft && mining){
		
		if (getTerritoryOwner(territory) != 21){
			mining = 0;
		}
		
		if (territory == inSection(getOpponentPosition())){
			mining = 0;
		}
		
		driveForwardFeet(5./12., -100);
		turnToPoint(sheepDispensers[territory - 1]);
		
		driveForwardFeet(calculateDistance(sheepDispensers[territory - 1], getMyPosition()) - 5.2/12., 100);
		
		servo_set_pos(1, LEVER_ARM_DOWN);
		pause(450);
		servo_set_pos(1, LEVER_ARM_UP);
		pause(450);
		miningLiningUpTries++;
		
		if (miningLiningUpTries > 3 && getTerritorySheepLeft(territory) == sheepLeft){
			mining = 0;
		}
	}
	
	while(getTerritorySheepLeft(territory) > 0 && mining){
		servo_set_pos(1, LEVER_ARM_DOWN);
		pause(450);
		servo_set_pos(1, LEVER_ARM_MIDDLE);
		pause(450);
		
		miningTries++;
		
		if (miningTries > 4){
			
			driveForwardFeet(8./12., -100);
			turnToPoint(sheepDispensers[territory - 1]);
			servo_set_pos(1, LEVER_ARM_UP);
			driveForwardFeet(calculateDistance(sheepDispensers[territory - 1], getMyPosition()) - 5.2/12., 100);
			miningTries = 0;
		}
		
		if (getTerritoryOwner(territory) != 21){
			mining = 0;
		}
	}
	
	driveForwardFeet(8./12., -150);
	
}



void spinWheel(){
	//printf("\nspinning the wheel");
	
	
	uint32_t start_time = get_time();
	uint8_t spinTries = 0;
	getData();
	uint8_t mySection = inSection(getMyPosition());
	int16_t spinDir = 1;

	// timeout, adjust to get gears while (object[]), use get_time() - start_time to check how long it's been, 
	// reset after each timeout, count timeouts, change actions
	if(getMyColor()){
		spinDir = -1;
	}	
	
	motor_set_vel(GEAR_MOTOR, spinDir*SPIN_VELOCITY);
	driveForwardFeet( 1.0/12., 75);
	
	while (getTerritoryOwner(mySection) != 21 && spinTries < 3){
		driveForwardFeet( 1.0/12., 50);
		
		if (get_time() - start_time > 3000){
			motor_set_vel(GEAR_MOTOR, 0);
			driveForwardFeet(4./12., -100);
			//driveToPoint(gatherTargetClose[mySection - 1], 0.5/12.);
			//driveToPoint(section[mySection -1], 0.5/12.);
			turnToPoint(gearBoxes[mySection - 1]);
			
			pause(100);
			turnToPoint(gearBoxes[mySection - 1]);
			
			motor_set_vel(GEAR_MOTOR, spinDir*SPIN_VELOCITY);
			driveForwardFeet((calculateDistance(gearBoxes[mySection - 1], getMyPosition()) - 2./12.), 100);
			start_time = get_time();
			spinTries ++;
		}
	}

	motor_set_vel(GEAR_MOTOR, 0);
	driveForwardFeet(12.0/12.0, -100);
	
}


void gatherSheep(uint8_t territory){
	//printf("getting sheeeeep");
	if (getTerritoryOwner(territory) != 21){
		captureTerritory(territory);
		spinWheel();
	}
	
	if(getTerritorySheepLeft(territory) > 0){
		getSheep(territory);
	}
}

void driveToNextPoint(uint8_t goalSection){
	uint8_t mySection = inSection(getMyPosition());
	
	int8_t difference = goalSection - mySection;
	int8_t smallGoal;
	
	if(difference < 0){
		difference = difference + 6;
	}
	if (difference > 3) {
		smallGoal = mySection -1;
		if(smallGoal <= 0){
			smallGoal = smallGoal + 6;
		}
	}else {
		smallGoal = mySection + 1;
		if (smallGoal > 6){
			smallGoal = smallGoal - 6;
		}
	}
	
	driveToPoint(section[smallGoal - 1], 3.0/12.);
}



void driveThroughFieldToPoint(Point goal, float accuracy){

	uint8_t goalSection = inSection(goal);
	
	while(!(goalSection == inSection(getMyPosition()))){
		driveToNextPoint(goalSection);
	}
	driveToPoint(goal, accuracy);
}



